
#include "Atm128I2C.h"

/**
 * This driver implements an interupt driven I2C Slave controller 
 * Hardware Abstraction Layer (HAL) to the ATmega128 
 * two-wire-interface (TWI) hardware subsystem.
 *
 * @author Laszlo Siroki
 *
 */

module Atm128I2CSlaveByteP {
	provides interface AsyncStdControl;
	provides interface Atm128I2CSlave as I2CSlave;
	uses interface HplAtm128I2CBus as I2C;
	uses interface Leds as ReadDebugLeds;
	uses interface Leds as WriteDebugLeds;
}

implementation {

  enum {
    I2C_OFF          = 0,
    I2C_IDLE         = 1,
    I2C_BUSY         = 2,      
//    I2C_ADDR         = 3,
    I2C_DATA         = 4,
//    I2C_STARTING     = 5,
//    I2C_STOPPING     = 6,
  } atm128_i2c_state_t;

	uint8_t state = I2C_OFF;
//	i2c_flags_t packetFlags; 
//  uint8_t* packetPtr;
//  uint8_t packetLen;
//  uint8_t index;
//  uint16_t packetAddr;
	bool reading = FALSE;
	uint8_t slave_addr = 42|1;
	
	uint8_t dbgcnt = 2;
/*
  void i2c_abort(error_t err) {
    atomic {
      // Cycle the I2C
      call I2C.readCurrent();
      call I2C.enableInterrupt(FALSE);
      call I2C.enable(FALSE);
      call I2C.sendCommand();
      call I2C.readCurrent();
      call I2C.enable(TRUE);
      call I2C.sendCommand();
      state = I2C_IDLE;
  }
*/  
	async command error_t AsyncStdControl.start() {
		atomic {
			if (state == I2C_OFF) {
				call I2C.init(TRUE);
				TWAR = slave_addr;
				call I2C.readCurrent();
				call I2C.enable(TRUE);            //TWEN
				call I2C.enableAck(TRUE);         //TWEA
				call I2C.enableInterrupt(TRUE);  //TWIE
				call I2C.setStart(FALSE);         //TWSTA
				call I2C.setStop(FALSE);          //TWSTO
				call I2C.sendCommand();
				state = I2C_IDLE;
				call WriteDebugLeds.set(1);
				return SUCCESS;
			} else {
				return FAIL;
			}
		}
	}

	async command error_t AsyncStdControl.stop() {
		atomic {
			if (state == I2C_IDLE) {
				call I2C.readCurrent();
				call I2C.enable(FALSE);
				call I2C.enableInterrupt(FALSE);
				call I2C.setInterruptPending(FALSE);
//				call I2C.off();
				state = I2C_OFF;
				return SUCCESS;
			} else {
				return FAIL;
			}
		}
	}

	command error_t I2CSlave.setSlaveAddress(uint8_t addr, bool generalCall) {
		if (generalCall)
			slave_addr = (addr&0xFE)|1;
		else
			slave_addr = (addr&0xFE);
		TWAR = slave_addr;
		return SUCCESS;
	}
  
  /**
   * A command has been sent over the I2C.
   */
	async event void I2C.commandComplete() {
		call I2C.readCurrent();
		call ReadDebugLeds.set(call I2C.status());

//		if (--dbgcnt == 0)
//			atomic { for(;;);}

		atomic {
			if (state == I2C_IDLE) {
				if (call I2C.status() == ATM128_I2C_SR_SLA_ACK ||
					call I2C.status() == ATM128_I2C_SR_GC_ACK) {   // Write operation (slave receive)
					state = I2C_DATA;
					reading = FALSE;  // master write
					call I2C.setInterruptPending(TRUE);  // TWINT
					call I2C.enableAck(TRUE);            //TWEA
					signal I2CSlave.startWrite();
					call I2C.sendCommand();
				} else if (call I2C.status() == ATM128_I2C_ST_SLA_ACK) {
					state = I2C_DATA;
					reading = TRUE;  // master read

//call ReadDebugLeds.set(call ReadDebugLeds.get()|(1<<0));

					call I2C.setInterruptPending(TRUE);  // TWINT
					call I2C.enableAck(TRUE);            //TWEA
					signal I2CSlave.startRead();

					signal I2CSlave.readRequest(); // should call I2CSlave.read()
				} else
					;//error
			} else if (state == I2C_DATA) {
				if (reading == TRUE) { // master read
					if (call I2C.status() == ATM128_I2C_ST_DATA_ACK) {
						call I2C.setInterruptPending(TRUE);  // TWINT
						call I2C.enableAck(TRUE);            //TWEA

call ReadDebugLeds.set(call ReadDebugLeds.get()|(1<<2));
						signal I2CSlave.readRequest(); // should call I2CSlave.read()

					} else if (call I2C.status() == ATM128_I2C_ST_DATA_NACK) {

call ReadDebugLeds.set(call ReadDebugLeds.get()|(1<<3));
						call I2C.setInterruptPending(TRUE);  // TWINT
						call I2C.enableAck(TRUE);            //TWEA
						call I2C.sendCommand();
						signal I2CSlave.stopRead();
						state = I2C_IDLE;
					}
				} else { //master write

call ReadDebugLeds.set(call I2C.status());
					if (call I2C.status() == ATM128_I2C_SR_SLA_DATA_ACK ||
						call I2C.status() == ATM128_I2C_SR_GC_DATA_ACK) {
						call I2C.setInterruptPending(TRUE);  // TWINT
						if (signal I2CSlave.write(call I2C.read()))
							call I2C.enableAck(TRUE);            //TWEA
						else
							call I2C.enableAck(FALSE);           //TWEA = 0 -> last byte
						call I2C.sendCommand();
					} else if (call I2C.status() == ATM128_I2C_SR_SLA_DATA_NACK ||
							call I2C.status() == ATM128_I2C_SR_GC_DATA_NACK) {
						call I2C.setInterruptPending(TRUE);  // TWINT
						if (signal I2CSlave.write(call I2C.read()))
							call I2C.enableAck(TRUE);            //TWEA
						else
							call I2C.enableAck(FALSE);           //TWEA = 0 -> last byte
						call I2C.sendCommand();
					} else if (call I2C.status() == ATM128_I2C_SR_START_STOP) {
						call I2C.setInterruptPending(TRUE);  // TWINT
						call I2C.enableAck(TRUE);            //TWEA
						call I2C.sendCommand();
						signal I2CSlave.stopWrite();
						state = I2C_IDLE;
					}
				}
			}
		}
	}

	async command void I2CSlave.read(uint8_t data) {
call ReadDebugLeds.set(call ReadDebugLeds.get()|(1<<1));
		call I2C.write(data);
		call I2C.sendCommand();
	}
/*
  async command void Atm128I2C.stop() {
    atomic {
      call I2C.readCurrent();
      call I2C.enableInterrupt(FALSE);
      call I2C.setStop(TRUE);
      call I2C.setInterruptPending(TRUE);
      call I2C.sendCommand();
    }
  }
  */
}
